#include <iostream>
#include "apm_flight_handler.h"

using namespace std;
using namespace cv;


#define APM_UART					"/dev/ttyACM0"
#define APM_UART_BAUDRATE   		"230400"
#define SBUF_SIZE 					512
#define GCS_MAVLINK_CHAN			1
#define CIRCULAR_BUFFER_SIZE		(4 * 1024)

//255 broadcast,
#define MAVLINK_SYSTEM_ID  			255  // SYSID_MYGCS  AND SYSID_ENFORECE
#define MAVLINK_COMPONENT_ID 		191  //3588 control board

#define FLIGHT_SYSTEM_ID			1
#define FLIGHT_COMPONENT_ID			1


static const uint32_t MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE =
        POSITION_TARGET_TYPEMASK_X_IGNORE |
        POSITION_TARGET_TYPEMASK_Y_IGNORE |
        POSITION_TARGET_TYPEMASK_Z_IGNORE;

static const uint32_t MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE =
        POSITION_TARGET_TYPEMASK_VX_IGNORE |
        POSITION_TARGET_TYPEMASK_VY_IGNORE |
        POSITION_TARGET_TYPEMASK_VZ_IGNORE;

static const uint32_t MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE =
        POSITION_TARGET_TYPEMASK_AX_IGNORE |
        POSITION_TARGET_TYPEMASK_AY_IGNORE |
        POSITION_TARGET_TYPEMASK_AZ_IGNORE;

static const uint32_t MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE =
        POSITION_TARGET_TYPEMASK_YAW_IGNORE;
static const uint32_t MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE =
        POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE;



int64_t getTimeMillis()
{
	int64_t timeMs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();

	return timeMs;
}


void sleepMs(uint32_t timeMs)
{
	usleep(timeMs * 1000);
}

double wrap360(const double angle)
{
    double res = fmod(angle, 360.0);
    if (res < 0)
    {
        res += 360.0;
    }
    return res;
}


static void siyiVideoCallbackHandler(cv::Mat image)
{
	flightHandler.setLiveViewImage(image);
}


static void siyiAttitudeCallbackHandler(float roll, float pitch, float yaw)
{
	flightHandler.setGimbalAttitude(roll, pitch, yaw);
}


apmFlightHandler::apmFlightHandler():
	m_systemTimeSetFlag(false),
	m_getLogDirFlag(false),
	m_ttyFd(-1),
	m_connect(false),
	m_logFp(NULL),
	m_lastLogNum(0),
	m_mavlinkRunFlag(false),
	m_flightMode(STABILIZE),
	m_flightArm(false),
	m_attitudeLogCount(0),
	m_landPointOk(false)
{
	m_mavlinkSendBuffer.m_buffer = NULL;
}


apmFlightHandler::~apmFlightHandler()
{
	m_mavlinkRunFlag = false;
	if(m_ttyFd > 0)
	{
		close(m_ttyFd);
		m_ttyFd = -1;
	}

	freeCircularBuf(&m_mavlinkSendBuffer);


	if(NULL != m_logFp)
	{
		fflush(m_logFp);
		fclose(m_logFp);
		m_logFp = NULL;
	}
}


void *mavlinkRecvRun(void *arg)
{
	flightHandler.handlerMavlinkRecv();
	return NULL;
}

void *mavlinkSendRun(void *arg)
{
	flightHandler.handlerMavlinkSend();
	return NULL;
}


bool apmFlightHandler::startUpSystem(MountSiyi *mountSiyiHandler)
{
	bool ret;

	// start hb log
	ret = getLogDir();
	if(!ret)
	{
		cout << "getLogDir failed" << endl;
	}
	else
	{
		ret = createLogFile();
		if(!ret)
		{
			cout << "createLogFile failed" << endl;
		}
	}

	//link uart
	m_ttyFd = ttyConfigure(APM_UART, APM_UART_BAUDRATE);
	if(m_ttyFd < 0)
	{
		writeLogFile("open %s error\n", APM_UART);
		return false;
	}

	if(mountSiyiHandler == NULL)
	{
		writeLogFile("mountSiyiHandler = null !!!\n");
		//return false;
	}
	else
	{
		if(mountSiyiHandler != NULL)
		{
			m_mountSiyiHandler = mountSiyiHandler;

			//register siyi function.
			m_mountSiyiHandler->registerSiyiVideoCallback(siyiVideoCallbackHandler);
			m_mountSiyiHandler->registerSiyiAttitudeCallback(siyiAttitudeCallbackHandler);

			writeLogFile("mountSiyiHandler is ok\n");
		}
	}

	//test mavlink message
	uint8_t buf[512];
	int count=30; // wait  30 seconds

	mavlink_message_t msg;
	do{
		sleepMs(1000);
		// receive tty data.
		bzero(buf, sizeof(buf));
		int len = read(m_ttyFd, buf, sizeof(buf));

		for(int i=0; i<len; i++)
		{
			if(mavlink_parse_char(GCS_MAVLINK_CHAN, buf[i], &msg, &m_mavlinkStatus))
			{
				switch(msg.msgid)
				{
					case MAVLINK_MSG_ID_HEARTBEAT:
					{
						m_connect = true;
						//Subscribe to mavlink messages
						subcribeMavlinkMessages();
						break;
					}
					default:
						break;
				}
			}
		}

	}while(count-- && !m_connect);

	if(false == m_connect)
	{
		writeLogFile("test mavlink message failed\n");
		return false;
	}

	if(initCircularBuf(&m_mavlinkSendBuffer, CIRCULAR_BUFFER_SIZE) < 0)
	{
		writeLogFile("send initCircularBuf failed\n");
		return false;
	}

	int threadRet = pthread_create(&m_mavlinkRecvRunTid, NULL, mavlinkRecvRun, NULL);
	if(0 != threadRet)
	{
		writeLogFile("mavlink recv pthread_create err\n");
		return false;
	}

	threadRet = pthread_create(&m_mavlinkSendRunTid, NULL, mavlinkSendRun, NULL);
	if(0 != threadRet)
	{
		writeLogFile("mavlink send  pthread_create err\n");
		return false;
	}

	m_mavlinkRunFlag = true;

	return true;
}

int apmFlightHandler::ttyConfigure(char *ttyPort, char *baudrate)
{
	int ttyFd;
	struct termios  oldTio = {0};
	struct termios newCfg = {0};
	int rate;

	assert(ttyPort != NULL && baudrate != NULL);

	ttyFd = open(ttyPort, O_RDWR | O_NOCTTY );

	if(ttyFd < 0 )
	{
		cout << "open " << ttyPort << "error" << endl;
		writeLogFile("open %s error\n", ttyPort);
		return -1;
	}

	if(tcgetattr(ttyFd, &oldTio) != 0)
	{
		cout << "tcgettattr failed" << endl;
		writeLogFile("tcgettattr failed \n");
		close(ttyFd);
		return -1;
	}

	if(strcmp(baudrate, "38400") == 0)
	{
		rate = B38400;
	}
	else if(strcmp(baudrate, "57600") == 0)
	{
		rate = B57600;
	}
	else if(strcmp(baudrate, "115200") == 0)
	{
		rate = B115200;
	}
	else if(strcmp(baudrate, "230400") == 0)
	{
		rate = B230400;
	}
	else
	{
		cout << "baud rate:" << baudrate << "is not support!" << endl;
		writeLogFile("baud rate %s is not support!\n", baudrate);
		close(ttyFd);
		return -1;
	}

	//set the raw model
	cfmakeraw(&newCfg);
	newCfg.c_cflag |= CREAD;

	if(cfsetspeed(&newCfg, rate) < 0)
	{
		cout << "cfsetispeed error:" << baudrate << endl;
		writeLogFile("cfsetispeed error %s\n", baudrate);
	}

	newCfg.c_cflag &= ~CSIZE;
	newCfg.c_cflag |= CS8;
	newCfg.c_cflag &= ~PARENB;
	newCfg.c_iflag &= ~INPCK;
	newCfg.c_cflag &= ~CSTOPB;
	newCfg.c_cc[VTIME] = 0;
	newCfg.c_cc[VMIN] = 0;

	if (tcflush(ttyFd, TCIOFLUSH) < 0)
	{
		cout << "tcflush error" << endl;
		writeLogFile("tcflush error\n");
		return -1;
	}

	if(tcsetattr(ttyFd, TCSANOW, &newCfg) < 0)
	{
		cout << "tcsetattr failed" << endl;
		writeLogFile("tcsetattr failed \n");
		close(ttyFd);
		return -1;
	}

	return ttyFd;
}


void apmFlightHandler::subcribeMavlinkMessages()
{
/*
	MAV_DATA_STREAM_RAW_SENSORS include:
			MSG_RAW_IMU,
			MSG_SCALED_IMU2,
			MSG_SCALED_IMU3,
			MSG_SCALED_PRESSURE,
			MSG_SCALED_PRESSURE2,
			MSG_SCALED_PRESSURE3,
	MAV_DATA_STREAM_EXTENDED_STATUS include:
			MSG_SYS_STATUS,
			MSG_POWER_STATUS,
			MSG_MCU_STATUS,
			MSG_MEMINFO,
			MSG_CURRENT_WAYPOINT, // MISSION_CURRENT
			MSG_GPS_RAW,
			MSG_GPS_RTK,
			MSG_GPS2_RAW,
			MSG_GPS2_RTK,
			MSG_NAV_CONTROLLER_OUTPUT,
			MSG_FENCE_STATUS,
			MSG_POSITION_TARGET_GLOBAL_INT,
	MAV_DATA_STREAM_RC_CHANNELS include:
			MSG_SERVO_OUTPUT_RAW,
			MSG_RC_CHANNELS,
			MSG_RC_CHANNELS_RAW,
	MAV_DATA_STREAM_RAW_CONTROLLER include:

	MAV_DATA_STREAM_POSITION include:
			MSG_LOCATION,
			MSG_LOCAL_POSITION

	MAV_DATA_STREAM_EXTRA1 include:
			MSG_ATTITUDE,
			MSG_SIMSTATE,
			MSG_AHRS2,
			MSG_PID_TUNING

	MAV_DATA_STREAM_EXTRA2 include:
			MSG_VFR_HUD

	MAV_DATA_STREAM_EXTRA3 include:
			MSG_AHRS,
			MSG_HWSTATUS,
			MSG_SYSTEM_TIME,
			MSG_WIND,
			MSG_RANGEFINDER,
			MSG_DISTANCE_SENSOR,
#if AP_TERRAIN_AVAILABLE
			MSG_TERRAIN,
#endif
			MSG_BATTERY2,
			MSG_BATTERY_STATUS,
			MSG_GIMBAL_DEVICE_ATTITUDE_STATUS,
			MSG_OPTICAL_FLOW,
			MSG_MAG_CAL_REPORT,
			MSG_MAG_CAL_PROGRESS,
			MSG_EKF_STATUS_REPORT,
			MSG_VIBRATION,
#if AP_RPM_ENABLED
			MSG_RPM,
#endif
			MSG_ESC_TELEMETRY,
			MSG_GENERATOR_STATUS,
			MSG_WINCH_STATUS,
#if HAL_EFI_ENABLED
			MSG_EFI_STATUS,
#endif

*/

	//subcribe STREAM_RAW_SENSORS
	StartMavlinkMessagesSend(1, MAV_DATA_STREAM_RAW_SENSORS);
 
	//subcribe STREAM_EXTENDED_STATUS
	StartMavlinkMessagesSend(5, MAV_DATA_STREAM_EXTENDED_STATUS);

	//subcribe STREAM_POSITION
	StartMavlinkMessagesSend(5, MAV_DATA_STREAM_POSITION);

	//subcribe STREAM_EXTRA1
	StartMavlinkMessagesSend(50, MAV_DATA_STREAM_EXTRA1);

	//subcribe STREAM_EXTRA3
	StartMavlinkMessagesSend(50, MAV_DATA_STREAM_EXTRA3);
}


void apmFlightHandler::StartMavlinkMessagesSend(uint8_t message_rate, uint8_t stream_id)
{
	int len;
	uint8_t buf[512];
	mavlink_message_t msg;
	mavlink_request_data_stream_t subcribeParameter;

	//subcribe STREAM_RAW_SENSORS
	subcribeParameter.req_message_rate = message_rate;
	subcribeParameter.target_system = 1;
	subcribeParameter.target_component = 0;
	subcribeParameter.req_stream_id = stream_id;
	subcribeParameter.start_stop = 1;

	mavlink_msg_request_data_stream_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &subcribeParameter);

	len = 0;
	bzero(buf, sizeof(buf));
	len = packMavlinkData(&msg, buf);
	if(len > 0)
	{
		write(m_ttyFd, buf, len);
	}
}



bool apmFlightHandler::getMavlinkRunFlag()
{
	return m_mavlinkRunFlag;
}
void apmFlightHandler::setMavlinkRunFlag(bool status)
{
	m_mavlinkRunFlag = status;
}

int  apmFlightHandler::packMavlinkData(mavlink_message_t *msg, uint8_t *buffer)
{
	unique_lock<mutex> ulock(m_packMavlinkDataLock);
	msg->seq = m_mavlinkStatus.current_tx_seq;
	m_mavlinkStatus.current_tx_seq = m_mavlinkStatus.current_tx_seq + 1;
	return mavlink_msg_to_send_buffer(buffer, msg);
}

void apmFlightHandler::handlerHeartBeatMsg(mavlink_message_t &msg)
{
	int64_t currentTimeStamp = getTimeMillis();
	mavlink_heartbeat_t packet;
    mavlink_msg_heartbeat_decode(&msg, &packet);

	//set flight mode
	unique_lock<mutex> ulock1(m_flightModeUpdateLock);
	memcpy(&m_flightMode, &packet.custom_mode, sizeof(uint32_t));
	memcpy(&m_flightModeUpdateTimeStamp, &currentTimeStamp, sizeof(int64_t));
	ulock1.unlock();


	struct  BaseMode baseMode;
	memcpy(&baseMode, &packet.base_mode, sizeof(struct  BaseMode));

	//set arm or disarm status
	unique_lock<mutex> ulock2(m_flightArmUpdateLock);
	m_flightArm = baseMode.m_safetyArmedFlag;
	memcpy(&m_flightArmUpdateTimeStamp, &currentTimeStamp, sizeof(int64_t));
	ulock2.unlock();

	//set flight system status
	unique_lock<mutex> ulock3(m_flightSystemStatusUpdateLock);
	m_flightSystemStatus = (MAV_STATE)packet.system_status;
	memcpy(&m_flightSystemStatusUpdateTimeStamp, &currentTimeStamp, sizeof(int64_t));
	ulock3.unlock();

	// 1s fflush once.
	fflush(m_logFp);
	writeLogFile("------------------------------------\n");
	writeLogFile("flight mode:%d, arm status:%d, system status:%d, timestamp:%ld\n",
				m_flightMode, (int)m_flightArm, (int)m_flightSystemStatus, m_flightModeUpdateTimeStamp);
}

void apmFlightHandler::getFlightMode(uint32_t &flightMode, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_flightModeUpdateLock);
	memcpy(&flightMode, &m_flightMode, sizeof(uint32_t));
	memcpy(&updateTimeStamp, &m_flightModeUpdateTimeStamp, sizeof(int64_t));
}


void apmFlightHandler::getFlightArmStatus(bool &arm, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_flightArmUpdateLock);
	arm = m_flightArm;
	memcpy(&updateTimeStamp, &m_flightArmUpdateTimeStamp, sizeof(int64_t));
}

void apmFlightHandler::getFlightSystemStatus(MAV_STATE &systemStatus, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_flightSystemStatusUpdateLock);
	systemStatus = m_flightSystemStatus;
	memcpy(&updateTimeStamp, &m_flightSystemStatusUpdateTimeStamp, sizeof(int64_t));
}


void apmFlightHandler::handlerSystemTimeMsg(mavlink_message_t &msg)
{
	mavlink_system_time_t packet;
	mavlink_msg_system_time_decode(&msg, &packet);

	if((false == m_systemTimeSetFlag) && (packet.time_unix_usec > 0))
	{
		struct timeval tv;
		tv.tv_sec = packet.time_unix_usec / 1000000;
		tv.tv_usec = packet.time_unix_usec % 1000000;

		if (settimeofday(&tv, NULL) == -1)
		{
			writeLogFile("set time failed.\n");
		}
		else
		{
			m_systemTimeSetFlag = true;
			writeLogFile("set time ok.\n");
		}
	}

	writeLogFile("time_unix_usec :%ld, time_boot_ms:%d\n", packet.time_unix_usec, packet.time_boot_ms);
}

void apmFlightHandler::handlerGpsRawMsg(mavlink_message_t &msg)
{
	int64_t currentTimeStamp = getTimeMillis();
	mavlink_gps_raw_int_t packet;
	mavlink_msg_gps_raw_int_decode(&msg, &packet);

	unique_lock<mutex> ulock1(m_gpsFixTypeUpdateLock);
	m_gpsFixType = (GPS_FIX_TYPE)packet.fix_type;
	memcpy(&m_gpsFixTypeUpdateTimeStamp, &currentTimeStamp, sizeof(int64_t));
	ulock1.unlock();

	writeLogFile("gps time_usec:%ld, fix type:%d, hdop:%d, statellites count:%d, ground spped:%dcm/s, lat:%d, lon%d, alt:%fm, time stamp:%ld\n",
					packet.time_usec, m_gpsFixType, packet.eph, packet.satellites_visible, packet.vel, packet.lat, packet.lon, (float)packet.alt/1000.0f, currentTimeStamp);
}


void apmFlightHandler::handlerAttitudeMsg(mavlink_message_t &msg)
{
	int64_t currentTimeStamp = getTimeMillis();
	mavlink_attitude_t packet;
	mavlink_msg_attitude_decode(&msg, &packet);

	unique_lock<mutex> ulock(m_flightAttitudeUpdateLock);
	m_flightAttitude.x = ToDeg(packet.pitch);
	m_flightAttitude.y = ToDeg(packet.roll);
	// flight yaw
	m_flightAttitude.z = wrap360(ToDeg(packet.yaw));
	memcpy(&m_flightAttitudeUpdateTimeStamp, &currentTimeStamp, sizeof(int64_t));
	ulock.unlock();

	m_attitudeLogCount++;
	if(m_attitudeLogCount > 50)
	{
		m_attitudeLogCount = 0;
		writeLogFile(" attitude pitch:%f, roll:%f, yaw:%f, timeStamp:%ld\n",
							m_flightAttitude.x, m_flightAttitude.y, m_flightAttitude.z, m_flightAttitudeUpdateTimeStamp);
	}
}

void apmFlightHandler::getFlightAttitude(Vector3f &flightAttitude, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_flightAttitudeUpdateLock);
	memcpy(&flightAttitude, &m_flightAttitude, sizeof(Vector3f));
	memcpy(&updateTimeStamp, &m_flightAttitudeUpdateTimeStamp, sizeof(int64_t));
}

float apmFlightHandler::getFlightCurrentYaw()
{
	unique_lock<mutex> ulock(m_flightAttitudeUpdateLock);
	return m_flightAttitude.z;
}

void apmFlightHandler::handleLocalPositionNed(mavlink_message_t &msg)
{
	int64_t currentTimeStamp = getTimeMillis();
	mavlink_local_position_ned_t packet;
	mavlink_msg_local_position_ned_decode(&msg, &packet);

	//set local position
	unique_lock<mutex> ulock1(m_localPositionUpdateLock);
	m_localPosition.x = packet.x;
	m_localPosition.y = packet.y;
	m_localPosition.z = packet.z;
	memcpy(&m_localPositionUpdateTimeStamp, &currentTimeStamp, sizeof(int64_t));
	ulock1.unlock();

	//set local velocity
	unique_lock<mutex> ulock2(m_localVelocityUpdateLock);
	m_localVelocity.x = packet.vx;
	m_localVelocity.y = packet.vy;
	m_localVelocity.z = packet.vz;
	memcpy(&m_localVelocityUpdateTimeStamp,  &currentTimeStamp, sizeof(int64_t));
	ulock2.unlock();

	writeLogFile("local pos x:%f, y:%f, z:%fm; vel x:%f, y:%f, z:%f m/s; timeStamp:%ld\n",
				m_localPosition.x, m_localPosition.y, m_localPosition.z,
				m_localVelocity.x, m_localVelocity.y, m_localVelocity.z,
				currentTimeStamp);
}


void apmFlightHandler::getLocalPositon(Vector3f &localPosition, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_localPositionUpdateLock);
	memcpy(&localPosition, &m_localPosition, sizeof(Vector3f));
	memcpy(&updateTimeStamp, &m_localPositionUpdateTimeStamp, sizeof(int64_t));
}

void apmFlightHandler::getLocalVelocity(Vector3f &localVelocity, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_localVelocityUpdateLock);
	memcpy(&localVelocity, &m_localVelocity, sizeof(Vector3f));
	memcpy(&updateTimeStamp, &m_localVelocityUpdateTimeStamp, sizeof(int64_t));
}



void apmFlightHandler::handlerGlobalPositionMsg(mavlink_message_t &msg)
{
	int64_t currentTimeStamp = getTimeMillis();
	mavlink_global_position_int_t packet;
	mavlink_msg_global_position_int_decode(&msg, &packet);

	//set position
	unique_lock<mutex> ulock1(m_flightPositionUpdateLock);
	m_flightPosition.x = packet.lat;
	m_flightPosition.y = packet.lon;
	m_flightPosition.z = (float)packet.alt / 1000.0f; // m
	memcpy(&m_flightPositionUpdateTimeStamp, &currentTimeStamp, sizeof(int64_t));
	ulock1.unlock();

	//set velocity
	unique_lock<mutex> ulock2(m_flightVelocityUpdateLock);
	m_flightVelocity.x = (float)packet.vx / 100.0f; // m/s
	m_flightVelocity.y = (float)packet.vy / 100.0f; // m/s
	m_flightVelocity.z = (float)packet.vz / 100.0f; // m/s
	memcpy(&m_flightVelocityUpdateTimeStamp, &currentTimeStamp, sizeof(int64_t));
	ulock2.unlock();

	//set relative alt
	unique_lock<mutex> ulock3(m_flightRelativeAltUpdateLock);
	m_flightRelativeAlt = (float)packet.relative_alt / 1000.0f; // m
	memcpy(&m_flightRelativeAltUpdateTimeStamp, &currentTimeStamp, sizeof(int64_t));
	ulock3.unlock();

	//set vehicle heading
	unique_lock<mutex> ulock4(m_flightHeadingUpdateLock);
	m_flightHeading = (float)packet.hdg / 100.0f;
	memcpy(&m_flightHeadingUpdateTimeStamp, &currentTimeStamp, sizeof(int64_t));
	ulock4.unlock();

	writeLogFile("pos x:%d, y:%d, alt:%d; vel x:%f, y:%f, z:%f m/s; relateive alt:%f, heading:%f, timeStamp:%ld\n",
				m_flightPosition.x, m_flightPosition.y, m_flightPosition.z,
				m_flightVelocity.x, m_flightVelocity.y, m_flightVelocity.z,
				m_flightRelativeAlt, m_flightHeading, currentTimeStamp);
}


void apmFlightHandler::getGlobalPosition(Vector3i &flightPosition, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_flightPositionUpdateLock);
	memcpy(&flightPosition, &m_flightPosition, sizeof(Vector3i));
	memcpy(&updateTimeStamp, &m_flightPositionUpdateTimeStamp, sizeof(int64_t));
}

void apmFlightHandler::getGlobalVelocity(Vector3f &flightVelocity, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_flightVelocityUpdateLock);
	memcpy(&flightVelocity, &m_flightVelocity, sizeof(Vector3f));
	memcpy(&updateTimeStamp, &m_flightVelocityUpdateTimeStamp, sizeof(int64_t));
}

void apmFlightHandler::getRelativeAlt(float &flightRelativeAlt, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_flightRelativeAltUpdateLock);
	memcpy(&flightRelativeAlt, &m_flightRelativeAlt, sizeof(float));
	memcpy(&updateTimeStamp, &m_flightRelativeAltUpdateTimeStamp, sizeof(int64_t));
}

void apmFlightHandler::getFlgihtHeading(float &flightHeading, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_flightHeadingUpdateLock);
	memcpy(&flightHeading, &m_flightHeading, sizeof(float));
	memcpy(&updateTimeStamp, &m_flightHeadingUpdateTimeStamp, sizeof(int64_t));
}


void apmFlightHandler::handlerIsLandStaus(mavlink_message_t &msg)
{
	int64_t currentTimeStamp = getTimeMillis();
	mavlink_extended_sys_state_t packet;
	mavlink_msg_extended_sys_state_decode(&msg, &packet);

	unique_lock<mutex> ulock(m_landedStateUpdateLock);
	memcpy(&m_landedStateUpdateTimeStamp, &currentTimeStamp, sizeof(int64_t));
	m_landedState = (MAV_LANDED_STATE)packet.landed_state;
	ulock.unlock();
	writeLogFile("handlerIsLandStaus landstate:%u\n", packet.landed_state);
}

void apmFlightHandler::getLandStatus(MAV_LANDED_STATE &landStatus, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_landedStateUpdateLock);
	memcpy(&landStatus, &m_landedState, sizeof(MAV_LANDED_STATE));
	memcpy(&updateTimeStamp, &m_landedStateUpdateTimeStamp, sizeof(int64_t));
}


void apmFlightHandler::handlerGetFlightHome(mavlink_message_t &msg)
{
	int64_t currentTimeStamp = getTimeMillis();
	mavlink_home_position_t packet;
	mavlink_msg_home_position_decode(&msg, &packet);

	unique_lock<mutex> ulock(m_flightHomeUpdateLock);
	memcpy(&m_flightHomeUpdateTimeStamp, &currentTimeStamp, sizeof(int64_t));
	m_flightHome.x = (float)packet.latitude * 10e-7;
	m_flightHome.y = (float)packet.longitude * 10e-7;
	m_flightHome.z = (float)packet.altitude * 10e-3;
	ulock.unlock();

	writeLogFile("handlerGetFlightHome latitude:%f, longitude:%f, altitude:%f\n", m_flightHome.x, m_flightHome.y, m_flightHome.z);
}

void apmFlightHandler::getFlightHome(Vector3f &home, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_flightHomeUpdateLock);
	memcpy(&home, &m_flightHome, sizeof(Vector3f));
	memcpy(&updateTimeStamp, &m_flightHomeUpdateTimeStamp, sizeof(int64_t));
}


void apmFlightHandler::handlerPositionTargetLocalNED(mavlink_message_t &msg)
{
	int64_t currentTimeStamp = getTimeMillis();
	mavlink_position_target_local_ned_t packet;
	mavlink_msg_position_target_local_ned_decode(&msg, &packet);

	unique_lock<mutex> ulock(m_targetPostionDistanceLock);
	m_targetPostionDistance.x = packet.x;
	m_targetPostionDistance.y = packet.y;
	m_targetPostionDistance.z = packet.z;
	m_targetPostionDistanceUpdateTimeStamp = currentTimeStamp;
	ulock.unlock();
}


void apmFlightHandler::getTargetPositionDistance(Vector3f &distance, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_targetPostionDistanceLock);
	memcpy(&distance, &m_targetPostionDistance, sizeof(Vector3f));
	memcpy(&updateTimeStamp, &m_targetPostionDistanceUpdateTimeStamp, sizeof(int64_t));
}


void apmFlightHandler::handlerCommandAck(mavlink_message_t &msg)
{
	int64_t currentTimeStamp = getTimeMillis();
	mavlink_command_ack_t packet;
	mavlink_msg_command_ack_decode(&msg, &packet);
	//only write to log
	writeLogFile("command: %d execute result:%d, timeStamp:%ld\n", packet.command, packet.result, currentTimeStamp);
	printf("-----------command: %d execute result:%d, timeStamp:%ld\n", packet.command, packet.result, currentTimeStamp);

	switch(packet.command)
	{
		case MAVLINK_MSG_ID_SET_MODE:
			m_setModeCommandResult = (MAV_RESULT)packet.result;
			break;
		case MAVLINK_MSG_ID_COMMAND_LONG:
			m_setCommandLongResult = (MAV_RESULT)packet.result;
			break;
		case MAVLINK_MSG_ID_COMMAND_INT:
			m_setcommandInitResult = (MAV_RESULT)packet.result;
			break;
	}
}


bool apmFlightHandler::setFlightMode(uint32_t mode)
{
	int len;
	uint8_t buf[512];
	mavlink_message_t msg;
	mavlink_set_mode_t packet;
	memset(&packet, 0, sizeof(packet));
	packet.custom_mode = mode;
	packet.base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
	packet.target_system = FLIGHT_SYSTEM_ID;
	mavlink_msg_set_mode_encode(FLIGHT_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &packet);

	len = 0;
	bzero(buf, sizeof(buf));
	len = packMavlinkData(&msg, buf);

	m_setModeCommandResult = MAV_RESULT_ENUM_END;
	if(len > 0)
	{
		writeCircularBuf(&m_mavlinkSendBuffer, buf, len);
	}
	else
	{
		return m_setModeCommandResult;
	}

	//wait ack result. max waiting time 1 sencod
	int cnt = 100;
	do
	{
		sleepMs(10);
		if(m_setModeCommandResult != MAV_RESULT_ENUM_END)
		{
			break;
		}
	}while(cnt--);

	if((m_setModeCommandResult == MAV_RESULT_ENUM_END) && (m_flightMode == mode))
	{
		m_setModeCommandResult = MAV_RESULT_ACCEPTED;
	}
	return m_setModeCommandResult == MAV_RESULT_ACCEPTED;
}


bool apmFlightHandler::commandDoGimbalManagerPitchyawRetract()
{
	int len;
	uint8_t buf[512];
	mavlink_message_t msg;
	mavlink_command_long_t packet;
	memset(&packet, 0, sizeof(mavlink_command_long_t));
	packet.command = MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
	packet.param5 = GIMBAL_MANAGER_FLAGS_RETRACT;
	//gimbal device id.  0 is primary, 1 is 1st gimbal, 2 is 2nd gimbal, etc
	packet.param7 = 0;
	packet.target_system = FLIGHT_SYSTEM_ID;
	packet.target_component = FLIGHT_COMPONENT_ID;
	packet.confirmation = 0;
	mavlink_msg_command_long_encode(FLIGHT_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &packet);

	len = 0;
	bzero(buf, sizeof(buf));
	len = packMavlinkData(&msg, buf);

	m_setCommandLongResult = MAV_RESULT_ENUM_END;
	if(len > 0)
	{
		writeCircularBuf(&m_mavlinkSendBuffer, buf, len);
	}
	else
	{
		return m_setCommandLongResult;
	}

	//wait ack result. max waiting time 10 ms
	int cnt = 10;
	do
	{
		sleepMs(10);
		if(m_setCommandLongResult != MAV_RESULT_ENUM_END)
		{
			break;
		}
	}while(cnt--);

	return m_setCommandLongResult == MAV_RESULT_ACCEPTED;
}

bool apmFlightHandler::commandDoGimbalManagerPitchyawNeutral()
{
	int len;
	uint8_t buf[512];
	mavlink_message_t msg;
	mavlink_command_long_t packet;
	memset(&packet, 0, sizeof(mavlink_command_long_t));
	packet.command = MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
	packet.param5 = GIMBAL_MANAGER_FLAGS_NEUTRAL;
	//gimbal device id.  0 is primary, 1 is 1st gimbal, 2 is 2nd gimbal, etc
	packet.param7 = 0;
	packet.target_system = FLIGHT_SYSTEM_ID;
	packet.target_component = FLIGHT_COMPONENT_ID;
	packet.confirmation = 0;
	mavlink_msg_command_long_encode(FLIGHT_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &packet);

	len = 0;
	bzero(buf, sizeof(buf));
	len = packMavlinkData(&msg, buf);

	m_setCommandLongResult = MAV_RESULT_ENUM_END;
	if(len > 0)
	{
		writeCircularBuf(&m_mavlinkSendBuffer, buf, len);
	}
	else
	{
		return m_setCommandLongResult;
	}

	//wait ack result. max waiting time 10 ms
	int cnt = 10;
	do
	{
		sleepMs(10);
		if(m_setCommandLongResult != MAV_RESULT_ENUM_END)
		{
			break;
		}
	}while(cnt--);

	return m_setCommandLongResult == MAV_RESULT_ACCEPTED;
}

bool apmFlightHandler::commandDoGimbalManagerPitchyawYawLock(float pitchAngleDeg, float yawAngleDeg, float pitchRateDegs, float yawRateDegs)
{
	int len;
	uint8_t buf[512];
	mavlink_message_t msg;
	mavlink_command_long_t packet;
	memset(&packet, 0, sizeof(mavlink_command_long_t));
	packet.command = MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
	packet.param1 = pitchAngleDeg;
	packet.param2 = yawAngleDeg;
	packet.param3 = pitchRateDegs;
	packet.param4 = yawRateDegs;
	packet.param5 = GIMBAL_MANAGER_FLAGS_YAW_LOCK;
	//gimbal device id.  0 is primary, 1 is 1st gimbal, 2 is 2nd gimbal, etc
	packet.param7 = 0;
	packet.target_system = FLIGHT_SYSTEM_ID;
	packet.target_component = FLIGHT_COMPONENT_ID;
	packet.confirmation = 0;
	mavlink_msg_command_long_encode(FLIGHT_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &packet);

	len = 0;
	bzero(buf, sizeof(buf));
	len = packMavlinkData(&msg, buf);

	m_setCommandLongResult = MAV_RESULT_ENUM_END;
	if(len > 0)
	{
		writeCircularBuf(&m_mavlinkSendBuffer, buf, len);
	}
	else
	{
		return m_setCommandLongResult;
	}

	//wait ack result. max waiting time 10 ms
	int cnt = 10;
	do
	{
		sleepMs(10);
		if(m_setCommandLongResult != MAV_RESULT_ENUM_END)
		{
			break;
		}
	}while(cnt--);

	return m_setCommandLongResult == MAV_RESULT_ACCEPTED;
}



//500hz recv
void apmFlightHandler::handlerMavlinkRecv()
{
	uint8_t buf[512];
	mavlink_message_t msg;

	//wait for init over  m_mavlinkRunFlag == true
	sleepMs(100);

	while(m_mavlinkRunFlag)
	{
		sleepMs(2);
		int len;

		do{
			len = 0;
			bzero(buf, sizeof(buf));
			len = read(m_ttyFd, buf, sizeof(buf));

			if(len < 0)
			{
				writeLogFile("handlerMavlinkRecv error\n");
			}

			for(int i=0; i<len; i++)
			{
				if(mavlink_parse_char(GCS_MAVLINK_CHAN, buf[i], &msg, &m_mavlinkStatus))
				{
					//copter sysid = 1, compid = 1
					if(msg.sysid == FLIGHT_SYSTEM_ID && msg.compid == FLIGHT_COMPONENT_ID)
					{
						//cout << "msgid:" << msg.msgid << ",sysid:" <<(int)msg.sysid << ",compid:" << (int)msg.compid << endl;
						switch(msg.msgid)
						{
							case MAVLINK_MSG_ID_HEARTBEAT: // 0
							{
								m_connect = true;
								handlerHeartBeatMsg(msg);
								break;
							}

							case MAVLINK_MSG_ID_SYSTEM_TIME: // 2
							{
								handlerSystemTimeMsg(msg);
								break;
							}

							case MAVLINK_MSG_ID_GPS_RAW_INT: // 24
							{
								handlerGpsRawMsg(msg);
							}

							case MAVLINK_MSG_ID_ATTITUDE: // 30
							{
								handlerAttitudeMsg(msg);
								break;
							}

							case MAVLINK_MSG_ID_LOCAL_POSITION_NED: // 32
							{
								handleLocalPositionNed(msg);
								break;
							}

							case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: // 33
							{
								handlerGlobalPositionMsg(msg);
								break;
							}

							case MAVLINK_MSG_ID_COMMAND_ACK: // 77
							{
								handlerCommandAck(msg);
								break;
							}

							case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED: // 85
							{
								handlerPositionTargetLocalNED(msg);
								break;
							}

							case MAVLINK_MSG_ID_HOME_POSITION: // 242
							{
								handlerGetFlightHome(msg);
								break;
							}

							case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: // 245  landstatus
							{
								handlerIsLandStaus(msg);
								break;
							}

							default:
								break;
						}
					}
				}
			}
		}while(len == sizeof(buf));
	}
}


//500 hz send
void apmFlightHandler::handlerMavlinkSend()
{
	uint8_t buf[512];
	//wait for init over  m_mavlinkRunFlag == true
	sleepMs(100);

	while(m_mavlinkRunFlag)
	{
		sleepMs(2);
		int len;
		do
		{
			len =0;
			bzero(buf, sizeof(buf));
			len = readCircularBuf(&m_mavlinkSendBuffer, buf, sizeof(buf));
			if(len > 0)
			{
				int ret = write(m_ttyFd, buf, len);

				if(ret < 0)
				{
					writeLogFile("handlerMavlinkSend write error\n");
				}
			}
		}while(len == sizeof(buf));
	}
}

bool apmFlightHandler::getLogDir()
{
	if (0 == access("./HbLogs", F_OK))
	{
		m_getLogDirFlag = true;
		return true;
	}
	else
	{
		int ret = mkdir("./HbLogs", 0777);

		if(0 == ret)
		{
			m_getLogDirFlag = true;
			return true;
		}
		else
		{
			m_getLogDirFlag = false;
			return false;
		}
	}

	return false;
}

bool apmFlightHandler::createLogFile()
{
	if(!m_getLogDirFlag)
	{
		return false;
	}

	for(int i=1; i<1000; i++)
	{
		char logName[30];
		memset(logName, 0, sizeof(logName) / sizeof(logName[0]));
		sprintf(logName,"./HbLogs/hb_logs_%d.txt", i);
		m_lastLogNum = i;
		if(0 == access(logName, F_OK))
		{
			continue;
		}
		else
		{
			if(999 == i)
			{
				if(0 == system("rm  ./HbLogs/*  -rf"))
				{
					m_logFp =  fopen("./HbLogs/hb_logs_1.txt", "ab+");
					if(NULL == m_logFp)
					{
						m_lastLogNum = 0;
						cout << "createlogFile failed1" << endl;
						return false;
					}
					m_lastLogNum = 1;
					return true;
				}
				else
				{
					m_lastLogNum--;
					cout << "rm ./HbLogs/* -rf failed" << endl;
					m_logFp = NULL;
					return false;
				}
			}
			else
			{
				m_logFp =  fopen(logName, "ab+");
				if(NULL == m_logFp)
				{
					m_lastLogNum--;
					cout << "createLogFile failed2" << endl;
					return false;
				}
				return true;
			}
		}
	}

	return false;
}


bool apmFlightHandler::writeLogFile(const char * format, ...)
{
	size_t size, n;
	char sbuf[SBUF_SIZE];

	if(NULL == m_logFp)
	{
		return false;
	}

	va_list args;
	va_start (args, format);
	n = vsnprintf (sbuf,SBUF_SIZE,format, args);
	va_end (args);

	size = fwrite(sbuf, 1, n, m_logFp);
    if (size != n) {
        fclose(m_logFp);
		m_logFp = NULL;
		cout << "writeLogFile fialed" << endl;
        return false;
    }

	return true;
}


bool apmFlightHandler::clearAllLogFile()
{
	if(m_logFp != NULL)
	{
		fclose(m_logFp);
		m_logFp = NULL;
	}

	if(0 == system("rm  ./HbLogs/*  -rf"))
	{
		m_lastLogNum = 0;
		//restart log
		createLogFile();
		return true;
	}
	cout << "clear all logFile failed!!!" << endl;
	return false;
}


bool apmFlightHandler::viewLogFile(vector<string> &logFileName)
{
	DIR* logDir = NULL;
	logDir = opendir("./HbLogs");

	if(NULL == logDir)
	{
		cout << "opendir ./HbLogs failed!!!" << endl;
		return false;
	}

	struct dirent* dirInfo;
	while((dirInfo = readdir(logDir)) != NULL)
	{
		if(strcmp(".", dirInfo->d_name) != 0 && strcmp("..", dirInfo->d_name) != 0)
		{
			logFileName.push_back(dirInfo->d_name);
		}
	}

	closedir(logDir);
	return true;
}

void apmFlightHandler::recodeCpuTemperature()
{
	FILE *file = fopen("/sys/class/thermal/thermal_zone0/temp", "r");
	if(NULL != file)
	{
		char buf[10];
		float temp;
		memset(buf, 0, 10);
		size_t len = fread(buf, 1, 10, file);
		if(len > 0)
		{
			temp  = atof(buf);
			writeLogFile("cpu temperature is %f\n", temp / 1000.0f);
		}
		fclose(file);
	}
}



//siyi mount
void apmFlightHandler::setGimbalAttitude(float roll, float pitch, float yaw)
{
	int64_t currentTimeStamp = getTimeMillis();

	unique_lock<mutex> ulock(m_siyiGimbalAttitudeLock);
	m_siyiGimbalAttitudeUpdateTimeStamp = currentTimeStamp;
	m_siyiGimbalAttitude.x = roll;
	m_siyiGimbalAttitude.y = pitch;
	m_siyiGimbalAttitude.z = yaw;
	ulock.unlock();

	writeLogFile("siyi gimbal roll:%f, pitch:%f ,yaw:%f\n", roll, pitch, yaw);
}

void apmFlightHandler::getGimbalAttitude(Vector3f &gimbalAttitude, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_siyiGimbalAttitudeLock);
	memcpy(&updateTimeStamp, &m_siyiGimbalAttitudeUpdateTimeStamp, sizeof(int64_t));
	memcpy(&gimbalAttitude, &m_siyiGimbalAttitude, sizeof(Vector3f));
}


void apmFlightHandler::setLiveViewImage(cv::Mat image)
{
	int64_t currentTimeStamp = getTimeMillis();

	unique_lock<mutex> ulock(m_siyiLiveViewLock);

	m_siyiLiveViewUpdateTimeStamp = currentTimeStamp;
	m_siyiLiveView = image.clone();

	int64_t updateTimeStamp;
	float flightHeading;
	//get current flight heading
	getFlgihtHeading(flightHeading, updateTimeStamp);
	m_imageFlightYaw = flightHeading;

	//get current gimbal yaw, gimbal is relative center angle
	getGimbalAttitude(m_siyiImageGimbalAngle, updateTimeStamp);
	m_siyiImageGimbalAngle.z += flightHeading;

	ulock.unlock();
	writeLogFile("setLiveViewImage flightHeading:%f, currentGimbaAngle:%f\n", flightHeading, m_siyiImageGimbalAngle);
}

void apmFlightHandler::getLiveViewImage(cv::Mat &image, float &imageFlightYaw, Vector3f& imageGimbalAngle, int64_t &updateTimeStamp)
{
	unique_lock<mutex> ulock(m_siyiLiveViewLock);
	image = m_siyiLiveView.clone();
	memcpy(&updateTimeStamp, &m_siyiLiveViewUpdateTimeStamp, sizeof(int64_t));
	memcpy(&imageFlightYaw, &m_imageFlightYaw, sizeof(float));
	memcpy(&imageGimbalAngle, &m_siyiImageGimbalAngle, sizeof(Vector3f));
}

bool apmFlightHandler::getLiveViewHealthy()
{
	if(m_mountSiyiHandler != NULL)
	{
		return m_mountSiyiHandler->getRtspHealthy();
	}

	return false;
}


bool apmFlightHandler::setPoistionControlFlight(int32_t latitude, int32_t longitude, float altitude)
{
	int len;
	uint8_t buf[512];
	mavlink_message_t msg;
	mavlink_command_int_t packet;
	memset(&packet, 0, sizeof(packet));
	packet.command = MAV_CMD_DO_REPOSITION;
	//global: latitude in degrees * 10^7
	packet.x = latitude;
	//global: longitude in degrees * 10^7
	packet.y = longitude;
	//global: altitude in meters (relative or absolute, depending on frame)
	packet.z = altitude;
	//MAV_FRAME_GLOBAL_RELATIVE_ALT is relativet
	packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;

	mavlink_msg_command_int_encode(FLIGHT_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &packet);

	len = 0;
	bzero(buf, sizeof(buf));
	len = packMavlinkData(&msg, buf);

	m_setcommandInitResult = MAV_RESULT_ENUM_END;
	if(len > 0)
	{
		writeCircularBuf(&m_mavlinkSendBuffer, buf, len);
	}
	else
	{
		return m_setcommandInitResult;
	}

	//wait ack result. max waiting time 10 ms
	int cnt = 10;
	do
	{
		sleepMs(10);
		if(m_setcommandInitResult != MAV_RESULT_ENUM_END)
		{
			break;
		}
	}while(cnt--);

	return m_setcommandInitResult == MAV_RESULT_ACCEPTED;
}



bool apmFlightHandler::setPoistionControlFlight2(int32_t latitude, int32_t longitude, float altitude)
{
	int len;
	uint8_t buf[512];
	mavlink_message_t msg;
	mavlink_set_position_target_global_int_t packet;
	memset(&packet, 0, sizeof(packet));

	packet.lat_int = latitude;
	packet.lon_int = longitude;
	packet.alt = altitude;
	packet.type_mask = MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE | MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE
						| MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE | MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
	packet.coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
	packet.target_system = MAVLINK_SYSTEM_ID;
	packet.target_component = MAVLINK_COMPONENT_ID;

	mavlink_msg_set_position_target_global_int_decode(&msg, &packet);

	len = 0;
	bzero(buf, sizeof(buf));
	len = packMavlinkData(&msg, buf);

	if(len > 0)
	{
		writeCircularBuf(&m_mavlinkSendBuffer, buf, len);
	}
	else
	{
		return false;
	}

	return true;
}


bool apmFlightHandler::setLandPoistionToFlight()
{
	if(!m_landPointOk)
	{
		return false;
	}

	return setPoistionControlFlight(m_landLatitude, m_landLongitude, m_landAltitude);
}



bool apmFlightHandler::velocityControlToCopter(Vector3f velocity, double yawRate)
{
	int len;
	uint8_t buf[512];
	mavlink_message_t msg;
	mavlink_set_position_target_global_int_t packet;
	memset(&packet, 0, sizeof(packet));

	packet.vx = velocity.x;
	packet.vy = velocity.y;
	packet.vz = velocity.z;
	packet.yaw_rate = yawRate;
	packet.coordinate_frame = MAV_FRAME_LOCAL_NED;
	packet.type_mask = MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE | MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE
						| MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
	packet.target_system = MAVLINK_SYSTEM_ID;
	packet.target_component = MAVLINK_COMPONENT_ID;

	mavlink_msg_set_position_target_global_int_decode(&msg, &packet);

	len = 0;
	bzero(buf, sizeof(buf));
	len = packMavlinkData(&msg, buf);

	if(len > 0)
	{
		writeCircularBuf(&m_mavlinkSendBuffer, buf, len);
	}
	else
	{
		return false;
	}

	return true;
}


apmFlightHandler flightHandler;
